#pragma once

#ifndef _IMU_HANDLE_H_
#define _IMU_HANDLE_H_

#include"utility.h"
#include"frame.h"

namespace esikf_slam{

class IMUHandle{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    typedef shared_ptr<IMUHandle> Ptr;

    struct IMUPose {
        double time;
        V3D un_acc;
        V3D un_gyr;
        V3D pos;
        V3D vel;
        Q4D q;
    };

    Config::Ptr p_config_;

    IMUHandle(){
        cov_acc_.setZero();
        cov_gyr_.setZero();
        cov_bias_acc_<<0.0001,0.0001,0.0001;
        cov_bias_gyr_<<0.0001,0.0001,0.0001;
    };
    IMUHandle(Config::Ptr p_config){
        p_config_ = p_config;
        cov_acc_ = p_config_->cov_acc_;
        cov_gyr_=p_config_->cov_gyr_;
        cov_bias_acc_=p_config_->cov_bias_acc_;
        cov_bias_gyr_=p_config_->cov_bias_gyr_;
    }

    void IMUInit(shared_ptr<Frame> p_frame);

    void predictBatch(shared_ptr<Frame> p_frame);

    void predict(shared_ptr<Frame> p_frame,V3D& acc_avr,V3D& angvel_avr,
                double dt,Eigen::Matrix<double,State::DIM_OF_STATE_,State::DIM_OF_STATE_>& Q);

private:
   V3D cov_acc_,cov_gyr_,cov_bias_gyr_,cov_bias_acc_;
   sensor_msgs::Imu::ConstPtr p_imu_last_;
   V3D init_acc_;
   vector<IMUPose> IMU_pose_v_;

   void undistortPoints(shared_ptr<Frame> p_frame);
   void addIMUPose(State state,sensor_msgs::ImuConstPtr imu_p);
};



}
#endif  
